// --------------------------------------------------------------------------------------------------------------------
// <copyright file="IKFilterTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Kinematics
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test IK filters
    /// </summary>
    [TestClass]    
    public class IKFilterTests
    {
        /// <summary>
        /// Tests least change filter
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsIKFilterWeightedLeastChangeTest()
        {
            List<DoubleArray> solutions = new List<DoubleArray>();

            DoubleArray reference = DoubleArray.From(new double[] { 0, 0, 0, 0, 0, 0, 0 });
            DoubleArray solution1 = DoubleArray.From(new double[] { 0, 1, 1, 1, 0, 0, 0 });
            DoubleArray solution2 = DoubleArray.From(new double[] { 0, 1, 1, 0, 0, 0, 0 });
            DoubleArray solution3 = DoubleArray.From(new double[] { 0, 1, 0, 0, 0, 0, 0 });

            // assembly solutions
            solutions.Add(solution1);
            solutions.Add(solution2);
            solutions.Add(solution3);
            
            // add duplicated solutions
            solutions.Add(solution2);
            solutions.Add(solution3);

            // Exp1: sort
            double[] scores;
            InverseKinematicsFilterWeightedLeastChange ikFilter = new InverseKinematicsFilterWeightedLeastChange(reference);
            List<DoubleArray> sortedSolutions = ikFilter.SortJointConfiguration(solutions, out scores);
            
            // verification
            Assert.AreEqual(sortedSolutions.Count, 5);
            CollectionAssert.AreEqual(solution3, sortedSolutions[0]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[1]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[2]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[3]);
            CollectionAssert.AreEqual(solution1, sortedSolutions[4]);
            
            Assert.AreEqual(scores.Length, 5);
            Assert.AreEqual(scores[0], 1);
            Assert.AreEqual(scores[1], 1);
            Assert.AreEqual(scores[2], 2);
            Assert.AreEqual(scores[3], 2);
            Assert.AreEqual(scores[4], 3);

            // Exp2: sort and clean up
            List<DoubleArray> cleanedUpSortedSolutions = ikFilter.SortJointConfigurationAndCleanUpDuplicated(solutions, out scores);

            // verification
            Assert.AreEqual(cleanedUpSortedSolutions.Count, 3);
            CollectionAssert.AreEqual(solution3, cleanedUpSortedSolutions[0]);
            CollectionAssert.AreEqual(solution2, cleanedUpSortedSolutions[1]);
            CollectionAssert.AreEqual(solution1, cleanedUpSortedSolutions[2]);
        }

        /// <summary>
        /// Tests weighted maximum margin filter
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsIKFilterWeightedMaximumMarginTest()
        {
            KinematicChain kc = KinematicChainTests.CreateA7DOFKinematicChain();

            List<DoubleArray> solutions = new List<DoubleArray>();

            DoubleArray solution1 = DoubleArray.From(new double[] { 0, 1, 1, 1, 0, 0, 0 });
            DoubleArray solution2 = DoubleArray.From(new double[] { 0, 1, 1, 0, 0, 0, 0 });
            DoubleArray solution3 = DoubleArray.From(new double[] { 0, 1, 0, 0, 0, 0, 0 });

            // assembly solutions
            solutions.Add(solution1);
            solutions.Add(solution2);
            solutions.Add(solution3);

            // add duplicated solutions
            solutions.Add(solution2);
            solutions.Add(solution3);

            // sort
            double[] scores;
            InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit ikFilter1 = new InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit(kc.JointList);
            List<DoubleArray> sortedSolutions = ikFilter1.SortJointConfiguration(solutions, out scores);

            // verification
            CollectionAssert.AreEqual(solution3, sortedSolutions[0]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[1]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[2]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[3]);
            CollectionAssert.AreEqual(solution1, sortedSolutions[4]);
            Assert.AreEqual(scores[0], -((1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) / (240 * 240 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians) + 6 * 0.25));
            Assert.AreEqual(scores[2], -((1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) / (240 * 240 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians) + (1 - 170 * MathConstants.Degrees2Radians) * (1 - 170 * MathConstants.Degrees2Radians) / (340 * 340 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians) + 5 * 0.25));
            Assert.AreEqual(scores[4], -(2 * (1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) / (240 * 240 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians) + (1 - 170 * MathConstants.Degrees2Radians) * (1 - 170 * MathConstants.Degrees2Radians) / (340 * 340 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians) + 4 * 0.25));

            // clean up
            List<DoubleArray> cleanedUpSortedSolutions = ikFilter1.SortJointConfigurationAndCleanUpDuplicated(solutions, out scores);

            // verification
            Assert.AreEqual(cleanedUpSortedSolutions.Count, 3);
            CollectionAssert.AreEqual(solution3, cleanedUpSortedSolutions[0]);
            CollectionAssert.AreEqual(solution2, cleanedUpSortedSolutions[1]);
            CollectionAssert.AreEqual(solution1, cleanedUpSortedSolutions[2]);

            // sort with weights
            double[] weights = { 1, 2, 3, 4, 1, 1, 1 };
            InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit ikFilter2 = new InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit(kc.JointList, false, weights);
            sortedSolutions = ikFilter2.SortJointConfiguration(solutions, out scores);

            // verification
            CollectionAssert.AreEqual(solution3, sortedSolutions[0]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[1]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[2]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[3]);
            CollectionAssert.AreEqual(solution1, sortedSolutions[4]);
            Assert.AreEqual(scores[0], -((1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) * 2 + (170 * 170 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians * 6 + 120 * 120 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians * 5)));
            Assert.AreEqual(scores[2], -((1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) * 2 + (1 - 170 * MathConstants.Degrees2Radians) * (1 - 170 * MathConstants.Degrees2Radians) * 3 + (170 * 170 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians * 3 + 120 * 120 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians * 5)));
            Assert.AreEqual(scores[4], -((1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) * 2 + (1 - 170 * MathConstants.Degrees2Radians) * (1 - 170 * MathConstants.Degrees2Radians) * 3 + (1 - 120 * MathConstants.Degrees2Radians) * (1 - 120 * MathConstants.Degrees2Radians) * 4 + (170 * 170 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians * 3 + 120 * 120 * MathConstants.Degrees2Radians * MathConstants.Degrees2Radians * 1)));
        }

        /// <summary>
        /// Tests absolute maximum margin filter
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsIKFilterAbsoluteMaximumMarginTest()
        {
            KinematicChain kc = KinematicChainTests.CreateA7DOFKinematicChain();

            List<DoubleArray> solutions = new List<DoubleArray>();

            DoubleArray solution1 = DoubleArray.From(new double[] { 0, 1, 1, 1, 0, 0, 0 });
            DoubleArray solution2 = DoubleArray.From(new double[] { 0, 1, 1, 0, 0, 0, 0 });
            DoubleArray solution3 = DoubleArray.From(new double[] { 0, 1, 0, 0, 0, 0, 0 });

            // assembly solutions
            solutions.Add(solution1);
            solutions.Add(solution2);
            solutions.Add(solution3);

            // add duplicated solutions
            solutions.Add(solution2);
            solutions.Add(solution3);

            // sort
            double[] scores;
            InverseKinematicsFilterAbsoluteMaximumMarginFromLimit ikFilter1 = new InverseKinematicsFilterAbsoluteMaximumMarginFromLimit(kc.JointList);
            List<DoubleArray> sortedSolutions = ikFilter1.SortJointConfiguration(solutions, out scores);

            // verification
            CollectionAssert.AreEqual(solution1, sortedSolutions[0]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[1]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[2]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[3]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[4]);
            Assert.AreEqual(scores[0], (1 - 120 * MathConstants.Degrees2Radians) / (240 * MathConstants.Degrees2Radians));
            Assert.AreEqual(scores[1], (1 - 120 * MathConstants.Degrees2Radians) / (240 * MathConstants.Degrees2Radians));
            Assert.AreEqual(scores[2], (1 - 120 * MathConstants.Degrees2Radians) / (240 * MathConstants.Degrees2Radians));

            // sort without normalization
            InverseKinematicsFilterAbsoluteMaximumMarginFromLimit ikFilter2 = new InverseKinematicsFilterAbsoluteMaximumMarginFromLimit(kc.JointList, false);
            sortedSolutions = ikFilter2.SortJointConfiguration(solutions, out scores);

            // verification
            CollectionAssert.AreEqual(solution1, sortedSolutions[0]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[1]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[2]);
            CollectionAssert.AreEqual(solution2, sortedSolutions[3]);
            CollectionAssert.AreEqual(solution3, sortedSolutions[4]);
            Assert.AreEqual(scores[0], (1 - 120 * MathConstants.Degrees2Radians));
            Assert.AreEqual(scores[1], (1 - 120 * MathConstants.Degrees2Radians));
            Assert.AreEqual(scores[2], (1 - 120 * MathConstants.Degrees2Radians));
        }

        /// <summary>
        /// Tests IK filter of maximum clearance from a plane
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsIKFilterMaximumClearanceFromPlaneTest()
        {
            KinematicChain kc = KinematicChainTests.CreateA7DOFKinematicChain();

            List<DoubleArray> solutions = new List<DoubleArray>();
            DoubleArray solution1 = DoubleArray.From(new double[] { 0, 1, 1, 1, 0, 0, 0 });
            DoubleArray solution2 = DoubleArray.From(new double[] { 0, 1, 1, 0, 0, 0, 0 });
            DoubleArray solution3 = DoubleArray.From(new double[] { 0, 1, 0, 0, 0, 0, 0 });
            solutions.Add(solution1.Transpose());
            solutions.Add(solution2.Transpose());
            solutions.Add(solution3.Transpose());

            // Look at the height of the last joint from the specified plane below
            bool[] elbowClearanceCheckList = { false, false, false, false, false, false, true };

            double[] scores;

            // consider the clearance of the last joint from the x-y plane at height z = 0
            InverseKinematicsFilter filter = new InverseKinematicsFilterMaximumClearanceFromPlane(kc, elbowClearanceCheckList, Vector3.ZAxis, new Vector3(0, 0, 0));
            List<DoubleArray> filteredSolution = filter.SortJointConfiguration(solutions, out scores);

            Matrix4[] transfs1 = kc.GetJointTransforms(filteredSolution[0]);
            Matrix4[] transfs2 = kc.GetJointTransforms(filteredSolution[1]);
            Matrix4[] transfs3 = kc.GetJointTransforms(filteredSolution[2]);

            Assert.IsTrue(transfs1[6][2, 3] >= transfs2[6][2, 3]);
            Assert.IsTrue(transfs2[6][2, 3] > transfs3[6][2, 3]);
        }
    }
}
